#include <iostream>
#include <iomanip>
#include "qbot_master.h"
#include "data_holder.h"
#include "serialport.h"

//#define DEBUG_MSG

QbotMaster::QbotMaster(SerialPort *_trans) : trans(_trans)
{
    memset(&battery_info, 0U, sizeof(Battery_T));
    recv_state = STATE_RECV_FIX;
    alive = true;
}

QbotMaster::~QbotMaster()
{
    trans->close();
    alive = false;
}

bool QbotMaster::init()
{
    if (!trans->open())
        return false;

    /* Start Io-service in Background, Run once */
    boost::thread([&]() { trans->run_forever(); });
    static boost::function0<void> _rf = boost::bind(&QbotMaster::recv_task, this);
    /* Start Message Sniff */
    this->recv_thread = boost::make_shared<boost::thread>(_rf);
    boost::thread([&]() { this->loop(); });
    return true;
}

void QbotMaster::loop()
{
    static int loop_cnt = 0;
    std::cout << "qbot master loop start" << std::endl;
    while(alive)
    {
        boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
        if(trans->ready)
        {
            interact(ID_GET_ROBOT_STATE);
            loop_cnt = 0;
        }
        else
        {
            if(loop_cnt++>10)
            {
                loop_cnt = 0;
                trans->open();
                std::cout << "[Warn] SerialPort Closed, Trying to ReOpen" << std::endl;
             }
        }
        
    }
}

void QbotMaster::msg_sniff()
{
    uint16_t data;
    uint8_t useful_data;
    trans->set_timeout(100);

    if (trans->available())
    {
        bool got = false;
        while (true)
        {
            if(trans->available())
            {
                data = trans->read();
                if(data < 0x0100)
                {
                    useful_data = data & 0x00FF;
                    if (data_recv(useful_data))
                    {
                        got = true;
                        break;
                    }
                }
            }

            if (trans->is_timeout()){
                    std::cout << "[read timeout]" << std::endl;
                    break;
                }
        }

        if (got)
            data_parse();
    }
}

void QbotMaster::recv_task()
{
    std::cout << "port receive thread start" << std::endl;
    while (alive)
    {
        //boost::this_thread::sleep_for(boost::chrono::milliseconds(1));
        this->msg_sniff();
    }
    std::cout << "port receive thread terminate" << std::endl;
}

/**
* @brief Only at the end of each frame return true
*/
bool QbotMaster::data_recv(unsigned char c)
{

#ifdef DEBUG_MSG
    if (c == FIX_HEAD)
        printf("[HEAD] ");
    printf("%02X ", c);
#endif

    switch (recv_state)
    {
    case STATE_RECV_FIX:
        if (c == FIX_HEAD)
        {
            memset(&active_rx_msg, 0, sizeof(active_rx_msg));
            active_rx_msg.head.flag = c;
            active_rx_msg.check += c;
            recv_state = STATE_RECV_ID;
        }
        else
            recv_state = STATE_RECV_FIX;
        break;
    case STATE_RECV_ID:
        if (c < ID_MESSGAE_MAX)
        {
            active_rx_msg.head.msg_id = c;
            active_rx_msg.check += c;
            recv_state = STATE_RECV_LEN;
        }
        else
            recv_state = STATE_RECV_FIX;
        break;
    case STATE_RECV_LEN:
        active_rx_msg.head.length = c;
        active_rx_msg.check += c;
        if (active_rx_msg.head.length == 0)
            recv_state = STATE_RECV_CHECK;
        else
            recv_state = STATE_RECV_DATA;
        break;
    case STATE_RECV_DATA:
        active_rx_msg.data[active_rx_msg.recv_count++] = c;
        active_rx_msg.check += c;
        if (active_rx_msg.recv_count >= active_rx_msg.head.length)
            recv_state = STATE_RECV_CHECK;
        break;
    case STATE_RECV_CHECK:
        if (active_rx_msg.check == c)
        {
            recv_state = STATE_RECV_END;
        }
        else
        {
            recv_state = STATE_RECV_FIX;
        }
        break;
    case STATE_RECV_END:
#ifdef DEBUG_MSG
        printf("[END]\n");
#endif
        recv_state = STATE_RECV_FIX;
        if (c == FIX_END)
        {
            return true;
        }
        break;
    default:
        recv_state = STATE_RECV_FIX;
        break;
    }

    return false;
}

bool QbotMaster::data_parse()
{
    MESSAGE_ID id = (MESSAGE_ID)active_rx_msg.head.msg_id;

#ifdef DEBUG_MSG
    printf("[data_parse]: id = %02X\r\n", id);
#endif

    DataHolder *dh = DataHolder::get();
    switch (id)
    {
    case ID_GET_VERSION:
        memcpy(&dh->firmware_info, active_rx_msg.data, sizeof(dh->firmware_info));
        dh->msg_flags.version = true;
        break;
    case ID_SET_ROBOT_PARAMTER:
        break;
    case ID_GET_ROBOT_PARAMTER:
        memcpy(&dh->parameter, active_rx_msg.data, sizeof(dh->parameter));
        break;
    case ID_INIT_ODOM:
        break;
    case ID_SET_VELOCITY:
        break;
    case ID_SET_POWER:
        break;
    case ID_GET_ODOM:
        memcpy(&dh->odom, active_rx_msg.data, sizeof(dh->odom));
        dh->msg_flags.odom = true;
        break;
    case ID_GET_ROBOT_STATE:
        memcpy(&dh->robot_state, active_rx_msg.data, sizeof(dh->robot_state));
        dh->msg_flags.robot_state = true;
        break;
    default:
        break;
    }

    return true;
}

bool QbotMaster::send_message(const MESSAGE_ID id)
{
    Message msg(id);
    send_message(&msg);
    return true;
}

bool QbotMaster::send_message(const MESSAGE_ID id, unsigned char *data, unsigned char len)
{
    Message msg(id, data, len);
    send_message(&msg);
    return true;
}

bool QbotMaster::send_message(Message *msg)
{
    if (trans == 0)
        return true;
    std::vector<uint8_t> data((unsigned char *)msg, (unsigned char *)msg + sizeof(msg->head) + msg->head.length + 2); //modified by MLC, add end byte
    trans->write(data);
    return true;
}

bool QbotMaster::interact(const MESSAGE_ID id)
{
    //clear buffer first, in case old data confuse receive
    //trans->clearReadbuffer();
    DataHolder *dh = DataHolder::get();
    switch (id)
    {
    case ID_GET_VERSION:
        send_message(id);
        break;
    case ID_SET_ROBOT_PARAMTER:
        send_message(id, (unsigned char *)&dh->parameter, sizeof(dh->parameter));
        break;
    case ID_GET_ROBOT_PARAMTER:
        send_message(id);
        break;
    case ID_INIT_ODOM:
        send_message(id);
        break;
    case ID_SET_VELOCITY:
        send_message(id, (unsigned char *)&dh->velocity, sizeof(dh->velocity));
        break;
    case ID_GET_ODOM:
        send_message(id);
        break;
    case ID_GET_PID_DATA:
        send_message(id);
        break;
    case ID_GET_ROBOT_STATE:
        send_message(id);
        break;
    case ID_GET_IMU_DATA:
        send_message(id);
        break;
    case ID_SET_EACH_MOTOR:
        send_message(id, (unsigned char *)&dh->each_motor, sizeof(dh->each_motor));
        break;
    case ID_RESET_WHEELS:
        send_message(id);
        break;
    case ID_SET_ODOM_TF:
        send_message(id, (unsigned char *)&dh->odom_tf, sizeof(dh->odom_tf));
        break;
    case ID_SET_POWER:
        send_message(id, (unsigned char *)&dh->power_onoff, sizeof(dh->power_onoff));
        break;
    default:
        break;
    }

    return true;
}